﻿using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Coding4Fun.Kinect.Wpf;
using Kinect.Toolbox.Voice;
using System.Windows.Threading;

namespace Kinectinator
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        SerialPort p;
        KinectSensor s;
        private double ArcAdjustment = 7;
        bool previouslyTracked = false;
        bool disarmed = true;
        double tiltcalibration = -15;
        private DateTime shootTime = DateTime.MaxValue;


        public MainWindow()
        {
            InitializeComponent();
        }

        private void Window_Loaded_1(object sender, RoutedEventArgs e)
        {
            p = new SerialPort("COM3", 9600, Parity.None, 8, StopBits.One);
            p.Open();

            /*
            Thread t = new Thread(new ThreadStart(() => 
            {
                while (true)
                {
                    string debugData = p.ReadLine();
                    System.Diagnostics.Debug.WriteLine(debugData);
                    //p.Write(new byte[] { 0xff, 60, 60 }, 0, 3);

                    //Thread.Sleep(1000);

                    //p.Write(new byte[] { 0xff, 230, 230 }, 0, 3);

                    //Thread.Sleep(1000);
                }
            }));

            t.Start();
            */


            SetupKinect();



            //soundLost.Play();
        }

        private void AimBlaster(double pan, double tilt)
        {
            tilt = tilt + tiltcalibration;
            byte panVal = (byte)((int)pan + 90);
            byte tiltVal = (byte)((int)tilt + 90);
            p.Write(new byte[] { 0xff, 0x01, tiltVal, panVal }, 0, 4);
        }

        private void ShootBlaster()
        {
            p.Write(new byte[] { 0xff, 0x02 }, 0, 2);
        }

        private void SetBlasterLedBrightness(byte redBrightness, byte greenBrightness, byte blueBrightness)
        {
            p.Write(new byte[] { 0xff, 0x03, redBrightness, greenBrightness, blueBrightness }, 0, 3);
        }

        private void SetupKinect()
        {
            s = KinectSensor.KinectSensors[0];
            s.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            s.ColorFrameReady += s_ColorFrameReady;

            //var tsp = new TransformSmoothParameters();
            s.SkeletonStream.Enable();
            //s.SkeletonStream.AppChoosesSkeletons = true;
            s.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default;
            s.SkeletonFrameReady += s_SkeletonFrameReady;

            s.Start();

            //voice command code
            //http://channel9.msdn.com/coding4fun/kinect/Templated-posture-detector-Voice-Commander-and-NuGet-package-now-available-in-v11-of-the-David-Catuh
            var voiceCommander = new VoiceCommander("arm", "disarm");
            voiceCommander.OrderDetected += voiceCommander_OrderDetected;
            voiceCommander.Start(s);

        }

        void voiceCommander_OrderDetected(string order)
        {
            switch (order)
            {
                case "arm":
                    disarmed = false;
                    Dispatcher.BeginInvoke(DispatcherPriority.Background, new Action(() => soundArm.Stop()));
                    Dispatcher.BeginInvoke(DispatcherPriority.Background, new Action(() => soundArm.Play()));
                    break;
                case "disarm":
                    disarmed = true;
                    Dispatcher.BeginInvoke(DispatcherPriority.Background, new Action(() => crosshair.Visibility = System.Windows.Visibility.Hidden));
                    Dispatcher.BeginInvoke(DispatcherPriority.Background, new Action(() => soundDisarm.Stop()));
                    Dispatcher.BeginInvoke(DispatcherPriority.Background, new Action(() => soundDisarm.Play()));
                    break;
            }
        }

        void s_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            using (var img = e.OpenColorImageFrame())
            {
                colorImageDisplay.Source = img.ToBitmapSource();
            }
        }

        void s_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            if (disarmed == false)
            {

                Skeleton[] skels;
                using (var skel = e.OpenSkeletonFrame())
                {
                    if (skel == null)
                    {
                        return;
                    }
                    skels = new Skeleton[skel.SkeletonArrayLength];
                    skel.CopySkeletonDataTo(skels);
                }
                var skel1 = skels.FirstOrDefault(s => s.TrackingState == SkeletonTrackingState.Tracked);
                if (skel1 != null && CheckSurrender(skel1) == false)
                {
                    crosshair.Visibility = System.Windows.Visibility.Visible;
                    var head = skel1.Joints[JointType.Head];
                    var shoulderCent = skel1.Joints[JointType.ShoulderCenter];
                    var spine = skel1.Joints[JointType.Spine];
                    var headScaled = head.ScaleTo((int)colorImageDisplay.Width, (int)colorImageDisplay.Height);

                    // find the center of the chest by averaging between the top of the shoulders and base of the spine
                    var target = new SkeletonPoint
                    {
                        X = (shoulderCent.Position.X + spine.Position.X) / 2,
                        Y = (shoulderCent.Position.Y + spine.Position.Y) / 2,
                        Z = (shoulderCent.Position.Z + spine.Position.Z) / 2
                    };

                    // var target = shoulderCent;

                    var screenPoint = SkeletonPointToScreen(target);
                    crosshair.SetValue(Canvas.LeftProperty, (colorImageDisplay.ActualWidth * screenPoint.X / 640) - (crosshair.ActualWidth / 2));
                    crosshair.SetValue(Canvas.TopProperty, (colorImageDisplay.ActualHeight * screenPoint.Y / 480) - (crosshair.ActualHeight / 2));

                    var footL = skel1.Joints[JointType.FootLeft];
                    var footR = skel1.Joints[JointType.FootRight];
                    Joint foot = new Joint();
                    bool hasFoot = false;
                    if (footL.TrackingState == JointTrackingState.Tracked)
                    {
                        foot = footL;
                        hasFoot = true;
                    }
                    else if (footR.TrackingState == JointTrackingState.Tracked)
                    {
                        foot = footR;
                        hasFoot = true;
                    }


                    //crosshair.Margin = new Thickness( - (crosshair.ActualWidth / 2),
                    //        (target.Position.Y * colorImageDisplay.Height) - (crosshair.ActualWidth / 2), 0, 0);

                    // now aim the blaster
                    AimBlaster(
                           RadToDeg(Math.Atan2(target.X, target.Z)),
                           RadToDeg(Math.Atan2(target.Y, target.Z)) + (target.Z * ArcAdjustment)
                           );
                    targetStatus.Content = "acquired";
                    distance.Content = string.Format("target range: {0:0.00} M", target.Z);
                    xLabel.Content = string.Format("x angle: {0:0.00} deg", RadToDeg(target.X));
                    yLabel.Content = string.Format("y angle: {0:0.00} deg", RadToDeg(target.Y));

                    if (hasFoot)
                    {
                        heightLabel.Content = String.Format("target ht: {0:0.00} M",
                                      Math.Abs(Math.Tan(foot.Position.Y) * foot.Position.Z) +
                                      Math.Abs(Math.Tan(head.Position.Y) * foot.Position.Z)
                                      );
                    }
                    else
                    {
                        heightLabel.Content = "unable to calc ht";
                    }

                    if (!previouslyTracked)
                    {
                        previouslyTracked = true;
                        soundDeploy.Stop();
                        soundDeploy.Play();
                    }


                    SetBlasterLedBrightness(0xfe, 0, 0);

                    //schedule shooting
                    if (shootTime == DateTime.MaxValue)
                    {
                        // schedule a shot
                        shootTime = DateTime.Now.AddSeconds(5);

                    }
                    firingLabel.Content = string.Format("firing in {0:0} ms", (shootTime - DateTime.Now).TotalMilliseconds);



                    // time to shoot?
                    if (shootTime < DateTime.Now)
                    {
                        //shoot!
                        soundFiring.Stop();
                        soundFiring.Play();

                        ShootBlaster();
                        shootTime = DateTime.MaxValue;
                    }
                }
                else
                {
                    targetStatus.Content = "searching";
                    crosshair.Visibility = System.Windows.Visibility.Hidden;
                    distance.Content = "...";
                    xLabel.Content = "...";
                    yLabel.Content = "...";
                    heightLabel.Content = "...";

                    // sleeping position
                    AimBlaster(0, 0);
                    if (previouslyTracked)
                    {
                        previouslyTracked = false;
                        soundLost.Stop();
                        soundLost.Play();
                    }

                    SetBlasterLedBrightness(0,0,0);

                    shootTime = DateTime.MaxValue; // don't shoot

                    firingLabel.Content = "...";
                }
            }
        }

        private Point SkeletonPointToScreen(SkeletonPoint skelpoint)
        {
            // Convert point to depth space.  
            // We are not using depth directly, but we do want the points in our 640x480 output resolution.
            DepthImagePoint depthPoint = s.CoordinateMapper.MapSkeletonPointToDepthPoint(skelpoint, DepthImageFormat.Resolution640x480Fps30);
            return new Point(depthPoint.X, depthPoint.Y);
        }

        bool CheckSurrender(Skeleton skeleton)
        {
            Joint shoulderLeftJoint = skeleton.Joints[JointType.ShoulderLeft];
            Joint shoulderRightJoint = skeleton.Joints[JointType.ShoulderRight];
            Joint leftHandJoint = skeleton.Joints[JointType.HandLeft];
            Joint rightHandJoint = skeleton.Joints[JointType.HandRight];

            if (leftHandJoint.Position.Y > shoulderLeftJoint.Position.Y && rightHandJoint.Position.Y > shoulderRightJoint.Position.Y)
            {
                soundSurrender.Stop();
                soundSurrender.Play();
                return true;
            }
            else
            {
                return false;
            }
        }

        public static double RadToDeg(double rad)
        {
            return rad * (180.0 / Math.PI);
        }
    }
   
}
